#include "Line.h"
#include "Circle.h"
#include <math.h>
#include "ContactManifold.h"
#include <Windows.h>
#include <iostream>
#include "Game.h"

Line::Line() : m_normal(Vector(0,0,0)), m_p0(Vector(0,0,0)), m_p1(Vector(0,0,0))
{
	m_data = new BodyInfo();
	m_data->m_mass = m_data->m_omega = 0.0f;
}

Line::Line(Vector p0, Vector p1)
{
	m_p0 = p0;
	m_p1 = p1;
	m_data = new BodyInfo();
	m_data->m_mass = m_data->m_omega = 0.0f;
}

Line::Line(Vector p0, Vector p1, Vector normal)
{
	m_p0 = p0;
	m_p1 = p1;
	m_normal = normal;
	m_data = new BodyInfo();
	m_data->m_mass = m_data->m_omega = 0.0f;
}

Line::~Line()
{

}

// Returns the vector from the line to the point with the smallest magnitude
Vector	Line::vectorTo(Vector p)
{
	return Vector(0,0,0);
}

void	Line::CollisionResponseWithCircle(ManifoldPoint& point, float dt)
{

	Circle* c1 = (Circle*)point.contactID1;
	Line* l = this;

	////////////////////////////////////////////////////////////////////////////


	//----------------------
	if (l->GetPos(0).GetX() >= -4.0f && l->GetPos(0).GetX() < 3.9f &&
		l->GetPos(0).GetY() <= 12.0f && l->GetPos(0).GetY() >= 0.0f)  {
			
			if (l->getNormal().dot(c1->GetPos()-l->GetPos(0)) > 0.0f && m_game->activeRope())
				m_game->rope().addForce(Vector(0, -c1->GetMass() * 9.81f));
	}

	//----------------------

	CollisionResponse(point, dt);
}

void	Line::CollisionResponseWithLine(ManifoldPoint& point, float dt)
{
}

void	Line::CollisionResponseWith(ManifoldPoint& point, float dt)
{
	point.contactID2->CollisionResponseWithLine(point, dt);
}


void	Line::CalculatePhysics(float dt)
{

}

void	Line::Update()
{

}

void	Line::SetPos(int i, float x, float y)
{
	if (0 == i)
		m_p0.Set(x, y);
	else  
		m_p1.Set(x, y);

	computeNormal();
}

void	Line::SetPos(float x0, float y0, float x1, float y1, float nx, float ny)
{
	m_p0.Set(x0, y0);
	m_p1.Set(x1, y1);
	m_normal.Set(nx, ny);
	m_normal.normalise();
}


void	Line::createSegments(int num, std::vector<Line>& vec)
{
	Vector dir = (m_p1-m_p0);

	for (int i=0; i<num; i++) {
		Vector p0 = m_p0 + dir * (float)i / (float)num;
		Vector p1 = m_p0 + dir * (float)(i+1) / (float)num;
		Line l(p0, p1, m_normal);
		vec.push_back(l);
	}
}

void	Line::updateCell()
{
	Vector mid = (m_p0 + m_p1) * 0.5f;

	int grid_size = 4;
	
	int x = (int)((mid.GetX() + 20.0f) / 4);
	int y = (int)((mid.GetY() + 20.0f) / 4);

	cell.Set((float)x, (float)y);			
}

void	Line::SetNormal(float x, float y)
{
	m_normal.Set(x,y);
	m_normal = m_normal.normalise();
}

void	Line::computeNormal()
{
	Vector line_normal = m_p1 - m_p0;
	
	m_normal.Set(line_normal.GetY(), line_normal.GetX());

	if (m_normal.GetX() > 0.0f || m_normal.GetX() < 0.0f)
		m_normal.Set(-m_normal.GetX(), m_normal.GetY());
	else
		m_normal.Set(m_normal.GetX(), -m_normal.GetY());

	m_normal = m_normal.normalise();
}


Vector	Line::GetPos(int i) const 
{ 
	if (i) 
		return m_p1; 
	else 
		return m_p0; 
}

float	Line::GetLength() const 
{ 
	return m_p0.distance(m_p1); 
}


void	Line::CollisionWithCircle(Circle* circle, ContactManifold *contactManifold, float dt)
{
	if (circle->getActive() == false)
		return;

	if (!active()) return;


	// A = accel.N
	float A = circle->GetAccel().dot(m_normal);
	// B = vel.N
	float B = circle->GetVel().dot(m_normal);
	// C = pos.N - b.N - r
	float C = circle->GetPos().dot(m_normal) - m_p0.dot(m_normal) - circle->GetRadius();

	float collision_t = -1.0f;
	Vector collision_point;


	
	if (A == 0.0f && B != 0.0f)	{
		float t = -C/B;
		if (t >= 0.0f && t <= dt) {
			collision_t = t;
			collision_point = circle->GetPos() - circle->GetRadius() * m_normal;
		}
	}
	else {

		float discriminent = B*B - 4.0f * A * C;
		if (discriminent < 0.0f)
			return;
		else if (discriminent - 0.0000001 < 0.0f) {
			float t = -B / (2.0f * A);
			if (t >= 0.0f && t <= dt) {
				collision_t = t;
			}
		}
		else	{
			float t0 = (-B + sqrt(discriminent)) / (2.0f * A);
			float t1 = (-B - sqrt(discriminent)) / (2.0f * A);

			//float collision_t= -1.0f;
			if (t0 >= 0.0f && t0 < dt)
				collision_t = t0;
			else if (t1 >= 0.0f && t1 < dt)
				collision_t = t1;

			collision_point = circle->GetPos() - circle->GetRadius() * m_normal;
		}

	}

	if (collision_t > 0.0f && belongsInLine(collision_point)) {
		ManifoldPoint point;
		point.impulse = 0.0f;
		point.contactID1 = this;
		point.contactID2 = circle;
		point.contactPoint = circle->GetPos() - circle->GetRadius() * m_normal;
		point.contactNormal = (circle->GetPos() - point.contactPoint).normalise();
		point.t = collision_t;
		point.t_impulse = 0;
		point.responded = false;

		#ifdef COMMON_RESPONSE
			point.contactID1 = circle;
			point.contactID2 = this;
		#endif

		contactManifold->Add(point);
		m_color.Set(m_color.GetY(), m_color.GetX());
	}
}


bool	Line::belongsInLine(Vector p)
{
	float d1 = m_p0.distance(p);
	float d2 = m_p1.distance(p);

	float d = m_p0.distance(m_p1);
	float dd = d1+d2;

	

	if (d1+d2 < d+0.8f)
		return true;

	return false;
}
